#include "system_config.h"

#include <stddef.h>

#ifdef HARMONY
#include "system/devcon/sys_devcon.h"
#include "system/ports/sys_ports.h"
#include "system/int/sys_int.h"
#else
#include <plib.h>
#endif

#include "PS2Protocol.h"
#include "CoreTimerDelay.h"

#define PS2DAT  _RB8        // PS2 Data input pin
#define PS2CLK  _RB7        // PS2 Clock input pin

// Scan Code states
#define PS2START    0
#define PS2BIT      1
#define PS2PARITY   2
#define PS2STOP     3
#define PS2PARITYN  4
#define PS2ACK      5

int tst;

// Variables
static int bitCount;
static int parity;
volatile static unsigned char scanCodeBuffer;
volatile static int PS2State;
volatile static bool scanCodeAvailable;
volatile static unsigned char scanCode;
volatile static int direction; // read = 1
volatile static int success;   // sent = 1 and ack = 0
volatile static int sent;
volatile static PS2_Callback_Function PS2callback = NULL;

void PS2_Init( void)
{
    CoreDelay_Init();
    scanCodeAvailable = false;
    direction = 1;
    PS2callback = NULL;

#ifdef HARMONY
    // Init pins 7,8 for input
    PLIB_PORTS_PinDirectionInputSet(PORTS_ID_0, PORT_CHANNEL_B, PORTS_BIT_POS_7);
    PLIB_PORTS_PinDirectionInputSet(PORTS_ID_0, PORT_CHANNEL_B, PORTS_BIT_POS_8);
    // Turn on change notification
    PLIB_PORTS_ChangeNoticePerPortTurnOn(PORTS_ID_0, PORT_CHANNEL_B);
    // Disable pullups since we have external resistor
    PLIB_PORTS_ChangeNoticePullUpPerPortDisable(PORTS_ID_0, PORT_CHANNEL_B, PORTS_BIT_POS_7);
    // Enable Port B Pin 7 for change notification
    PLIB_PORTS_PinChangeNoticePerPortEnable(PORTS_ID_0, PORT_CHANNEL_B, PORTS_BIT_POS_7);

    // Set up change notification interrupt
    PLIB_INT_SourceEnable(INT_ID_0, INT_SOURCE_CHANGE_NOTICE_B);
    PLIB_INT_VectorPrioritySet(INT_ID_0, INT_VECTOR_CN, INT_PRIORITY_LEVEL1);
    PLIB_INT_VectorSubPrioritySet(INT_ID_0, INT_VECTOR_CN, INT_SUBPRIORITY_LEVEL0);

#else
    // Init I/Os
    PORTSetPinsDigitalIn(IOPORT_B,BIT_7);
    PORTSetPinsDigitalIn(IOPORT_B,BIT_8);
    // Set up change notification
    mCNBOpen(CNB_ON, CNB7_ENABLE, CNB_PULLUP_DISABLE_ALL);
    ConfigIntCNB(CHANGE_INT_PRI_1 | CHANGE_INT_ON);
#endif
}

void DelayCallback(){
#ifdef HARMONY
    PLIB_PORTS_PinClear(PORTS_ID_0, PORT_CHANNEL_B, PORTS_BIT_POS_8);
    PLIB_PORTS_PinSet(PORTS_ID_0, PORT_CHANNEL_B, PORTS_BIT_POS_7);
    PLIB_PORTS_PinDirectionInputSet(PORTS_ID_0, PORT_CHANNEL_B, PORTS_BIT_POS_7);
    PLIB_PORTS_Read(PORTS_ID_0, PORT_CHANNEL_B);
    PLIB_PORTS_PinChangeNoticePerPortEnable(PORTS_ID_0, PORT_CHANNEL_B, PORTS_BIT_POS_7);
#else
    PORTClearBits(IOPORT_B, BIT_8);
    PORTSetBits(IOPORT_B, BIT_7);
    PORTSetPinsDigitalIn(IOPORT_B,BIT_7);
    mPORTBRead();
    EnableCNB7;
#endif
}

void PS2_Send(unsigned char data)
{
    scanCodeBuffer = data;
    scanCodeAvailable = false;
    direction = 0;
    success = 0;
    sent = 0;
    PS2State = PS2START;

#ifdef HARMONY    
    PLIB_PORTS_PinChangeNoticePerPortDisable(PORTS_ID_0, PORT_CHANNEL_B, PORTS_BIT_POS_7);
    PLIB_PORTS_PinClear(PORTS_ID_0, PORT_CHANNEL_B, PORTS_BIT_POS_7);  // start transmitting in 100us
    PLIB_PORTS_PinSet(PORTS_ID_0, PORT_CHANNEL_B, PORTS_BIT_POS_8);
    PLIB_PORTS_PinDirectionOutputSet(PORTS_ID_0, PORT_CHANNEL_B, PORTS_BIT_POS_7);
    PLIB_PORTS_PinDirectionOutputSet(PORTS_ID_0, PORT_CHANNEL_B, PORTS_BIT_POS_8);
#else
    DisableCNB7;
    PORTClearBits(IOPORT_B, BIT_7);       // start transmitting in 100us
    PORTSetBits(IOPORT_B, BIT_8);
    PORTSetPinsDigitalOut(IOPORT_B,BIT_8);
    PORTSetPinsDigitalOut(IOPORT_B,BIT_7);
#endif

    CoreDelay_SetDelayCallback(DelayCallback);
    CoreDelay_Async_us1(100);
}

#ifdef HARMONY
void ChangeNotificationPBInterrupt(){
#else
void __ISR( _CHANGE_NOTICE_VECTOR, ipl1) CNInterrupt( void){
#endif

    bool oneShot = false;
    if (direction){                     // Read Data
        if ( PS2CLK == 0)               // Falling Edge
        {
            switch( PS2State){
            default:
            case PS2START:              // verify start bit
                if ( ! PS2DAT)
                {
                    parity = 0xFF;     // init parity check
                    bitCount = 8;         // init bit counter
                    PS2State = PS2BIT;
                }
                break;
            case PS2BIT:
                scanCodeBuffer >>= 1;           // shift in data bit
                if ( PS2DAT)
                    scanCodeBuffer += 0x80;
                parity ^= scanCodeBuffer;       // update parity
                if ( --bitCount == 0)           // if all bit read, move on
                    PS2State = PS2PARITY;
                break;
            case PS2PARITY:
                if ( (PS2DAT << 7) == (parity & 0x80) )     // verify parity
                    PS2State = PS2STOP;
                else
                    PS2State = PS2START;
                break;
            case PS2STOP:
                if ( PS2DAT)            // verify stop bit
                {
                    scanCode = scanCodeBuffer;      // save code in mail box
                    scanCodeAvailable = true;       // set flag, code available
                    oneShot = true;
                }
                PS2State = PS2START;
                break;
            }
        }
    } else {                            // Send Data
        if (PS2CLK == 0){               // Falling clock edge
            // Only start and ack bits are needed on falling edge.
            switch( PS2State){
            case PS2START:
                bitCount = 8;           // Init bit counter
                parity = 0xFF;          // Init parity check
                PS2State = PS2BIT;      // Start sending data on rising edge
                break;
            case PS2ACK:
                sent = 1;
                if (!PS2DAT)
                {
                    success = 1;
                }
                // Reset to receive. 
                PS2State = PS2START;
                direction = 1;
                break;
            }
        }
        else {                          //Rising edge
            // All data bits and parity are set on the rising edge.
            // The pin is also set back to input on a rising edge.
            switch( PS2State){
            case PS2BIT:
                parity ^= scanCodeBuffer;       // update parity
                PS2DAT = scanCodeBuffer & 0x01;
                scanCodeBuffer >>= 1;           // shift out data bit
                if ( --bitCount == 0)           // if all bits read, move on
                    PS2State = PS2PARITY;
                break;
            case PS2PARITY:
                PS2DAT = parity;
                PS2State = PS2PARITYN;
                bitCount = 2;
                break;
            case PS2PARITYN:
                if ( --bitCount == 0)
#ifdef HARMONY
                    PLIB_PORTS_PinChangeNoticePerPortDisable(PORTS_ID_0, PORT_CHANNEL_B, PORTS_BIT_POS_7);
                    PLIB_PORTS_PinDirectionInputSet(PORTS_ID_0, PORT_CHANNEL_B, PORTS_BIT_POS_8);
                    PLIB_PORTS_Read(PORTS_ID_0, PORT_CHANNEL_B);
                    PLIB_PORTS_PinChangeNoticePerPortEnable(PORTS_ID_0, PORT_CHANNEL_B, PORTS_BIT_POS_7);
#else
                    // Set the pin so the keyboard can send ACK bit
                    DisableCNB7;
                    PORTSetPinsDigitalIn(IOPORT_B,BIT_8);
                    mPORTBRead();
                    EnableCNB7;
#endif
                    PS2State = PS2ACK;
                break;
            }
        }        
    }

    // clear interrupt flag
#ifdef HARMONY
    PLIB_INT_SourceFlagClear(INT_ID_0, INT_SOURCE_CHANGE_NOTICE_B);
#else
    mCNBClearIntFlag();
#endif

    if (PS2callback != NULL && oneShot){
        PS2callback();
    }
}

bool PS2_Available(){
    return scanCodeAvailable;
}

unsigned char PS2_GetScanCode(){
    if (scanCodeAvailable){
        scanCodeAvailable = false;
        return scanCodeBuffer;
    }
    return 0;
}

void PS2_AvailableCallback(PS2_Callback_Function function){
    PS2callback = function;
}
